#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QDoubleValidator>

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    //发送次数
    if(times==65535)
        times=0;
    //times+=1;
    times=0;

    udpSocket = new QUdpSocket(this);
    connect(udpSocket, &QUdpSocket::readyRead, this, &MainWindow::readMsg);//接收数据
    connect(udpSocket, &QUdpSocket::disconnected, this, &MainWindow::on_internet_disconnect_clicked);

    connect(ui->input_x, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->input_x_slider, [=](double v)
    {
        ui->input_x_slider->setValue(v);

    });
    connect(ui->input_x_slider, &QSlider::valueChanged, ui->input_x, [=](int v)
    {
        ui->input_x->setValue(v);
    });

    connect(ui->input_y, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->input_y_slider, [=](double v)
    {
        ui->input_y_slider->setValue(v);

    });
    connect(ui->input_y_slider, &QSlider::valueChanged, ui->input_y, [=](int v)
    {
        ui->input_y->setValue(v);
    });

    connect(ui->input_z, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->input_z_slider, [=](double v)
    {
        ui->input_z_slider->setValue(v);

    });
    connect(ui->input_z_slider, &QSlider::valueChanged, ui->input_z, [=](int v)
    {
        ui->input_z->setValue(v);
    });

    connect(ui->input_pitch, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->input_pitch_slider, [=](double v)
    {
        ui->input_pitch_slider->setValue(v);

    });
    connect(ui->input_pitch_slider, &QSlider::valueChanged, ui->input_pitch, [=](int v)
    {
        ui->input_pitch->setValue(v);
    });

    connect(ui->input_yaw, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->input_yaw_slider, [=](double v)
    {
        ui->input_yaw_slider->setValue(v);

    });
    connect(ui->input_yaw_slider, &QSlider::valueChanged, ui->input_yaw, [=](int v)
    {
        ui->input_yaw->setValue(v);
    });

    connect(ui->input_roll, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->input_roll_slider, [=](double v)
    {
        ui->input_roll_slider->setValue(v);

    });
    connect(ui->input_roll_slider, &QSlider::valueChanged, ui->input_roll, [=](int v)
    {
        ui->input_roll->setValue(v);
    });

    connect(ui->max_v, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->max_v_slider, [=](double v)
    {
        ui->max_v_slider->setValue(v);

    });
    connect(ui->max_v_slider, &QSlider::valueChanged, ui->max_v, [=](int v)
    {
        ui->max_v->setValue(v);
    });

    connect(ui->max_omega, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->max_omega_slider, [=](double v)
    {
        ui->max_omega_slider->setValue(v);
    });
    connect(ui->max_omega_slider, &QSlider::valueChanged, ui->max_omega, [=](int v)
    {
        ui->max_omega->setValue(v);
    });

    connect(ui->max_a, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->max_a_slider, [=](double v)
    {
        ui->max_a_slider->setValue(v);

    });
    connect(ui->max_a_slider, &QSlider::valueChanged, ui->max_a, [=](int v)
    {
        ui->max_a->setValue(v);
    });

    connect(ui->max_beta, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), ui->max_beta_slider, [=](double v)
    {
        ui->max_beta_slider->setValue(v);

    });
    connect(ui->max_beta_slider, &QSlider::valueChanged, ui->max_beta, [=](int v)
    {
        ui->max_beta->setValue(v);
    });

}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::on_internet_connect_clicked()
{
    host = ui->ip_server->text().trimmed();
    port = ui->ip_port->text().toInt();
    local_port=ui->ip_port_2->text().toInt();
    host = ui->ip_server->text();
    if(udpSocket->state() == QAbstractSocket::UnconnectedState){
        udpSocket->bind(QHostAddress::Any, local_port);
        ui->internet_connect->setText("重连");
    }
        local_ip = QHostAddress(ui->ip_local->text().trimmed());
        if(host.isEmpty() || port == 0)
        {
            QMessageBox::information(NULL, "警告", "请输入完整信息", QMessageBox::Yes);
            return;
        qDebug()<<host<<","<<port;
    }
}

void MainWindow::on_internet_disconnect_clicked()
{
    udpSocket->abort();
    ui->internet_connect->setText("连接");
    qDebug()<<"Disconnected";
}

//请求反馈数据（设定下为系统周期上传）
void MainWindow::readMsg()
{

    //有个问题：端口绑定后对方下线再上线，能发送数据但是不能接收数据？
    char buf[10240] = {0};//接收数据
    qint64 len = udpSocket->readDatagram(buf, sizeof(buf), &local_ip, &local_port);

    //这是最开始的做法
    QByteArray data = QByteArray::fromRawData(buf, len);

    //test
    //QByteArray data=QByteArray::fromStdString("66 66 66 66 00 05 2a ed 00 00 00 0b 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 43 34 f0 fb 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 44 3f 0b 90 44 3f 0b 90 44 44 b1 ea 44 39 e3 47 44 39 e3 47 44 44 b1 ea 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 88 88 88 88 ").trimmed();
    RECEIVE_FROM_PLC receive_from_plc=deserializeFrame(data);
        //判断字头和字尾
//    if(receive_from_plc.head!=0x66666666||receive_from_plc.tail!=0x88888888)
//    {
//        return;
//    }
//        qDebug()<<"receive:"<<data;
//    qDebug()<<"接收数据中";
    if(len < 0){
        qDebug()<<"接收数据失败！";
        return;
    }

    //ui->connected->setText("已连接");

    //接收数据,//状态字	=0 未启动；=1 初始化中；=2 初始化完成；=3 回零中；=4 已到零位；=5 负载定位中；=6 负载插入中；=7 负载安装到位；
    //			=8 平台退出中；=9急停；=10伺服故障；=11到极限位置指令无法下发；=12杆长到软限位;=13停止运动
    if(receive_from_plc.status==1){
        ui->command_zero->setText("初始化中");
    }
    else if(receive_from_plc.status==2){
        ui->command_zero->setText("初始化完成");
        //在这里判断是否连接成功合不合理？？？
        isConnected=true;
    }
    else if(receive_from_plc.status==3){
        ui->command_zero->setText("回零中");
    }
    else if(receive_from_plc.status==4){
        ui->command_zero->setText("已回零");
    }
    else if(receive_from_plc.status==5){
        ui->command_zero->setText("定位中");
    }
    else if(receive_from_plc.status==6){
        ui->command_zero->setText("插入中");
    }
    else if(receive_from_plc.status==7){
        ui->command_zero->setText("安装到位");
    }
    else if(receive_from_plc.status==8){
        ui->command_zero->setText("推出中");
    }
    else if(receive_from_plc.status==9){
        ui->command_zero->setText("急停");
    }
    else if(receive_from_plc.status==10){
        ui->command_zero->setText("故障");
    }
    else if(receive_from_plc.status==11){
        ui->command_zero->setText("角度软极限");
    }
    else if(receive_from_plc.status==12){
        ui->command_zero->setText("杆长软限位");
    }
    else if(receive_from_plc.status==13){
        ui->command_zero->setText("停止运动");
    }
    else if(receive_from_plc.status==14){
        ui->command_zero->setText("硬限位");
    }
    else if(receive_from_plc.status==15){
        ui->command_zero->setText("对准中");
    }
    else if(receive_from_plc.status==16){
        ui->command_zero->setText("对准完成");
    }
    else if(receive_from_plc.status==17){
        ui->command_zero->setText("弯矩或力过大");
    }

//        if(receive_from_plc.status==0){

//        }
//        if(receive_from_plc.status==1){

//        }
//        if(receive_from_plc.status==2){
//            //收到反馈数据
            //qDebug()<<"inlet";
        ui->status->display((int)receive_from_plc.status);
        ui->x->display(receive_from_plc.x);
        ui->y->display(receive_from_plc.y);
        ui->z->display(receive_from_plc.z);
        ui->roll->display(receive_from_plc.roll);
        ui->pitch->display(receive_from_plc.pitch);
        ui->yaw->display(receive_from_plc.yaw);

        ui->l1->display(receive_from_plc.l1);
        ui->l2->display(receive_from_plc.l2);
        ui->l3->display(receive_from_plc.l3);
        ui->l4->display(receive_from_plc.l4);
        ui->l5->display(receive_from_plc.l5);
        ui->l6->display(receive_from_plc.l6);



        float t1=receive_from_plc.torque[0]/16384*2.016/0.64*100;
        float t2=receive_from_plc.torque[1]/16384*2.016/0.64*100;
        float t3=receive_from_plc.torque[2]/16384*2.016/0.64*100;
        float t4=receive_from_plc.torque[3]/16384*2.016/0.64*100;
        float t5=receive_from_plc.torque[4]/16384*2.016/0.64*100;
        float t6=receive_from_plc.torque[5]/16384*2.016/0.64*100;

        float ave_t=(t1+t2+t3+t4+t5+t6)/6;


        b=0.99*b+0.01*ave_t;


        ui->torque1->display(t1);
        ui->torque2->display(t2);
        ui->torque3->display(t3);
        ui->torque4->display(t4);
        ui->torque5->display(t5);
        ui->torque6->display(t6);
        ui->torque_all->display(b);

        ui->f1->display(receive_from_plc.sensor1);
        ui->f2->display(receive_from_plc.sensor2);
        ui->f3->display(receive_from_plc.sensor3);

}

//启动/关闭电机：R1
void MainWindow::on_motor_on_clicked()
{
    if(ui->motor_on->text() == "启动电机"){
        SEND_TO_PLC send_to_plc;
        send_to_plc.command=1;
        ui->command_control->setText("使能");
        ui->control->display((int)send_to_plc.command);
        ui->motor_on->setText("关闭电机");
        //发送指令

        //if(读取到启动中)


        //if(读取到已启动）

        ui->motor1->setText("已启动");
        ui->motor2->setText("已启动");
        ui->motor3->setText("已启动");
        ui->motor4->setText("已启动");
        ui->motor5->setText("已启动");
        ui->motor6->setText("已启动");



//        /*test*/
//        QByteArray data;
//        QDataStream stream(&data, QIODevice::WriteOnly);

//        //发送数据需要知道目标主机的ip和端口号
//        udpSocket->writeDatagram(data, QHostAddress(host), port);

//        qDebug()<<"Send Data(R1):"<<data<<"\n";
//        /*test*/
    }
    else if(ui->motor_on->text() == "关闭电机"){
        ui->motor_on->setText("启动电机");

        ui->motor1->setText("已关闭");
        ui->motor2->setText("已关闭");
        ui->motor3->setText("已关闭");
        ui->motor4->setText("已关闭");
        ui->motor5->setText("已关闭");
        ui->motor6->setText("已关闭");
    }
}

//回零：R2
void MainWindow::on_pushButton_clicked()
{
    SEND_TO_PLC send;
    ui->command_control->setText("回零");
    stop=false;
    send.command=2;
    ui->control->display((int)send.command);
    QByteArray data=serializeFrame(send);//数据帧
    udpSocket->writeDatagram(data, QHostAddress(host), port);
    qDebug()<<"Send Data:"<<data;
}

//设置定位模式和目标位姿：R3
void MainWindow::on_input_clicked()
{
    SEND_TO_PLC send_to_plc;
    if(stop==true){
        QMessageBox::information(NULL, "警告", "请点击“继续运动”按钮！", QMessageBox::Yes);
        return;
    }

    QString mode;

    int param_coord=0;
    //模式选择
    if(ui->mode_absolute->isChecked()){
        mode=ui->mode_absolute->text()+"\n";
        param_mode=1;
    }
    else if(ui->mode_relative->isChecked()){
        mode=ui->mode_relative->text()+"\n";
        param_mode=2;
    }
    else if(ui->mode_force_and_position->isChecked()){
        mode=ui->mode_force_and_position->text()+"\n";
        param_mode=3;
    }
    //提示没有选择模式
    if(!ui->mode_absolute->isChecked()&&!ui->mode_relative->isChecked()&&!ui->mode_force_and_position->isChecked()){
        QMessageBox::information(NULL, "警告", "请选择定位模式", QMessageBox::Yes);
        return;
    }

    //坐标选择
    if(ui->base_coordinate->isChecked()){
        mode+=ui->base_coordinate->text();
        param_coord=1;
    }
    else if(ui->platform_coordinate->isChecked()){
        mode+=ui->platform_coordinate->text();
        param_coord=2;
    }

    if(!ui->base_coordinate->isChecked()&&!ui->platform_coordinate->isChecked()){
        QMessageBox::information(NULL, "警告", "请选择坐标模式", QMessageBox::Yes);
        return;
    }
    if(ui->input_x->text().isEmpty() || ui->input_y->text().isEmpty() || ui->input_z->text().isEmpty() ||
            ui->input_roll->text().isEmpty() || ui->input_pitch->text().isEmpty() || ui->input_yaw->text().isEmpty()){
        QMessageBox::information(NULL, "警告", "请输入完整数据", QMessageBox::Yes);
        return;
    }
    float roll_in=0.0f,pitch_in=0.0f;
    send_to_plc.command = 3;//指令类型
    send_to_plc.subCommand=param_mode;
    send_to_plc.subCommand2=param_coord;

    if(param_mode==1)
    {

        roll_in=ui->input_roll->text().toFloat();
        pitch_in=ui->input_pitch->text().toFloat();
    }

    else if(param_mode==2)
    {

        roll_in=ui->input_pitch->text().toFloat();
        pitch_in=ui->input_roll->text().toFloat();
    }
//    else
//    {printf("");}


    ui->command_control->setText("定位中");
    ui->control->display((int)send_to_plc.command);
    ui->subcommand->display(param_mode);
    ui->subcommand2->display(param_coord);
    float x_in=ui->input_x->text().toFloat();
    float y_in=ui->input_y->text().toFloat();
    float z_in=ui->input_z->text().toFloat();


    float yaw_in=ui->input_yaw->text().toFloat();

    send_to_plc.x=x_in;
    send_to_plc.y=y_in;
    send_to_plc.z=z_in;
    send_to_plc.roll=roll_in;
    send_to_plc.pitch=pitch_in;
    send_to_plc.yaw=yaw_in;
    ui->mode_current->setText(mode);
    QByteArray data=serializeFrame(send_to_plc);//数据帧

    udpSocket->writeDatagram(data, QHostAddress(host), port);
    qDebug()<<"Send Data:"<<data;
}

//停止运动：R8
void MainWindow::on_stop_clicked()
{
    SEND_TO_PLC send;
    stop=true;
    send.command=8;
    ui->control->display((int)send.command);
    ui->command_control->setText("停止运动");
    QByteArray data=serializeFrame(send);//数据帧
    udpSocket->writeDatagram(data, QHostAddress(host), port);
}

//继续运动：R9
void MainWindow::on_continue_2_clicked()
{
    SEND_TO_PLC send;
    stop=false;
    send.command=9;
    ui->command_control->setText("继续运动");
    ui->control->display((int)send.command);
    QByteArray data=serializeFrame(send);//数据帧
    udpSocket->writeDatagram(data, QHostAddress(host), port);

}

//自检：R
void MainWindow::on_self_check_clicked()
{
//    if(ui->is_checked->text()=="未自检")
//    {
        SEND_TO_PLC send;
        //RECIVE_FROM_PLC recive;
        send.command=8;
        //receive=


        ui->is_checked->setText("自检中");

        QByteArray data=serializeFrame(send);
        //发送数据需要知道目标主机的ip和端口号
        udpSocket->writeDatagram(data, QHostAddress(host), port);
        qDebug()<<"Send Data(R2):"<<data;

        //if(receive.???==???)//自检完成

        ui->is_checked->setText("已自检");
//    }
}

//设定最大速度、最大加速度、最大加速度、最大角加速度：R
void MainWindow::on_input_max_clicked()
{
    SEND_TO_PLC send_to_plc;
//    send_to_plc.command = 9;//设定最大值
    float max_v_in=ui->max_v->text().toFloat();
    float max_a_in=ui->max_a->text().toFloat();
    float max_omega_in=ui->max_omega->text().toFloat();
    float max_beta_in=ui->max_beta->text().toFloat();

    if(ui->max_v->text().isEmpty() || ui->max_a->text().isEmpty() || ui->max_omega->text().isEmpty() || ui->max_beta->text().isEmpty()){
        QMessageBox::information(NULL, "警告", "请输入完整数据", QMessageBox::Yes);
        return;
    }
//    send_to_plc.max_v=max_v_in;
//    send_to_plc.max_a=max_a_in;
//    send_to_plc.max_omega=max_omega_in;
//    send_to_plc.max_beta=max_beta_in;

//    QByteArray data=serializeFrame(send_to_plc);
//    //发送数据需要知道目标主机的ip和端口号
//    udpSocket->writeDatagram(data, QHostAddress(host), port);
//    qDebug()<<"Send Data(R6):"<<data;
}


QByteArray MainWindow::serializeFrame(SEND_TO_PLC frame)
{
    QByteArray data;
    QDataStream stream(&data, QIODevice::WriteOnly);
//    QDataStream::setFloatingPointPrecision(QDataStream::SinglePrecision);

    stream.setByteOrder(QDataStream::BigEndian);
    stream<<frame.head<<frame.count<<
            frame.command<<frame.subCommand<<frame.subCommand2;
    //QDataStream默认浮点型数据为8字节
    writeReal(stream,frame.x);
    writeReal(stream,frame.y);
    writeReal(stream,frame.z);
    writeReal(stream,frame.roll);
    writeReal(stream,frame.yaw);
    writeReal(stream,frame.pitch);
    stream<<frame.tail;
            //frame.max_v<<frame.max_omega<<frame.max_a<<frame.max_beta<<
    return data;
}

void MainWindow::on_clear_clicked()
{
    ui->input_x->setValue(0);
    ui->input_y->setValue(0);
    ui->input_z->setValue(0);
    ui->input_roll->setValue(0);
    ui->input_pitch->setValue(0);
    ui->input_yaw->setValue(0);
}

void MainWindow::writeReal(QDataStream& out, float f)
{
    out.setFloatingPointPrecision(QDataStream::SinglePrecision);
    out << f;
}

//模式选择→改变文本（俯仰→绕x，偏航→绕y，滚转→绕z）
void MainWindow::on_mode_absolute_toggled(bool checked)
{
    if(checked)
    {
        ui->position->setText("位置(mm)");
        ui->attitude->setText("姿态(°)");
        ui->pitch_box->setTitle("俯仰pitch");
        ui->yaw_box->setTitle("偏航yaw");
        ui->roll_box->setTitle("滚转roll");
        param_mode=1;
    }
}



void MainWindow::on_mode_relative_toggled(bool checked)
{
    if(checked)
    {
        ui->position->setText("位置增量(mm)");
        ui->attitude->setText("姿态增量(°)");
        ui->pitch_box->setTitle("绕x轴旋转");
        ui->yaw_box->setTitle("绕y轴旋转");
        ui->roll_box->setTitle("绕z轴旋转");
        param_mode=2;
    }
}

